#ifndef CPFLOCALIZER_H_
#define CPFLOCALIZER_H_

#include "Localizer.h"
#include "MapServer.h"
#include "MyPose.h"
#include "LNP.h"
#include "PoseArrayVisualizer.h"
#include "constants.h"
#include "ConditionalParticleFilter.h"
#include "cpf/TrackPos.h"
#include "PersonDetector.h"

class CPFLocalizer: public Localizer {
private:
	CPFSensorModel cpfsm;

	ros::Subscriber subm;
	ros::ServiceServer trackService;

	int Nr, Ny, M, readOdomInvoked;
	STLPose poseOfTarget;
	ConditionalParticleFilter cpf;
	PersonDetector pd;
	Robot config;


	void manPoseCallback(const geometry_msgs::PoseStamped &msg);
	void findMeansAndStdsOfPersons(PoseArray &means, vector<DD> &stds);
public:
	CPFLocalizer(ros::NodeHandle &n);
	virtual void readOdom(nav_msgs::Odometry msg);
	virtual void localize(OdomReading &last, OdomReading &current);
	bool Track(cpf::TrackPos::Request  &req, cpf::TrackPos::Response &res);
};

#endif /* CPFLOCALIZER_H_ */
